#pragma once

#include "vio/vio_common.h"

namespace vio {
namespace vision_utils {

struct HomographyDecomposition {
  Eigen::Vector3d t;
  Eigen::Matrix3d R;
  double d;
  Eigen::Vector3d n;
  // resolved composition
  Sophus::SE3d T;  // second from first
  int score;
};

class Homography {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Homography(const std::vector<Eigen::Vector2d,
                               Eigen::aligned_allocator<Eigen::Vector2d>>& fts1,
             const std::vector<Eigen::Vector2d,
                               Eigen::aligned_allocator<Eigen::Vector2d>>& fts2,
             double error_multiplier2,
             double thresh_in_px);
  void CalcFromPlaneParams(const Eigen::Vector3d& normal,
                           const Eigen::Vector3d& point_on_plane);
  void CalcFromMatches();

  size_t ComputeMatchesIniers();

  bool ComputeSE3FromMatches();

  bool Decompose();

  void FindBestDecomposition();

  double thresh_;
  double error_multiplier2_;
  const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>&
      fts_c1_;  // features on first image on unit plane
  const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>&
      fts_c2_;  // features on second image on unit plane
  std::vector<bool> inliers_;
  Sophus::SE3d
      T_c2_from_c1_;  // relative translation and rotation of two images
  Eigen::Matrix3d H_c2_from_c1_;  // homography
  std::vector<HomographyDecomposition> decompositions_;
};
}  // namespace vision_utils
}  // namespace vio